//
// Created by Lenovo on 2021/9/16.
//

#ifndef NLOPT_TEST_CARTPOLESYSTEM_H
#define NLOPT_TEST_CARTPOLESYSTEM_H
#include <Eigen/Dense>
#include <vector>
#include <casadi/casadi.hpp>
using namespace casadi;
struct GuessStruct
{
    std::vector<double> time;
    std::vector<Eigen::Matrix<double, 4,1> > state;
    std::vector<double> control;
};
struct BoundsStruct
{
    Eigen::Matrix<double, 4,1> initialState_lower;
    Eigen::Matrix<double, 4,1> initialState_upper;
    Eigen::Matrix<double, 4,1> finalState_lower;
    Eigen::Matrix<double, 4,1> finalState_upper;
    double initialTime_lower;
    double initialTime_upper;
    double finalTime_lower;
    double finalTime_upper;
    Eigen::Matrix<double, 4,1> state_lower;
    Eigen::Matrix<double, 4,1> state_upper;
    double control_lower;
    double control_upper;
};
class CartPoleSystem {
private:
    double m1 = 1.0;
    double m2 = 1.0;
    double g = 9.81;
    double l = 1.0;
    int N = 15;
public:
    double CartPoleInvDynT(double q, double dq, double ddq, double ddx);
    double CartPoleInvDynF(double q, double dq, double ddq, double ddx);
    MX cartPoleDynamicsMX0(MX x, MX q, MX dx, MX dq, MX F);
    MX cartPoleDynamicsMX1(MX x, MX q, MX dx, MX dq, MX F);
    MX cartPoleDynamicsMX2(MX x, MX q, MX dx, MX dq, MX F);
    MX cartPoleDynamicsMX3(MX x, MX q, MX dx, MX dq, MX F);
     
    GuessStruct computeGuess();
    BoundsStruct computeBounds(const GuessStruct& guess);
};


#endif //NLOPT_TEST_CARTPOLESYSTEM_H
